
#include <p33FJ16GS504.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "init.h"

extern unsigned char BEMFMODE;
extern unsigned char state;
extern unsigned char Step;
extern unsigned char align;
extern signed int MinSetpoint;
extern signed int Setpoint;
extern signed int SetpointOld;
extern signed int error;
extern unsigned char startcalculation;
extern unsigned int PIDset;
extern float Pvalue;
extern float Ivalue;
extern unsigned int errorcount;
extern unsigned int errorthreshold;
extern unsigned char Align;
extern unsigned int BEMFSTARTCOUNT;
extern unsigned int _BEMF_BUS;
extern unsigned int PIDStartCounter;
extern unsigned long CDELAY;


void stage_align()
{
	if ( Align==1 )
	{
		if ( MDC>2500 )
		{
			Align=0;
			//BEMF MA Rising
			IOCON1bits.OVRENH=1;//Override Enable for PWM1H Pin bit 0=PWM 1= OVD
			IOCON1bits.OVRENL=1; //Override Enable for PWM1L Pin bit 0=PWM 1= OVD
			IOCON1bits.OVRDAT1=0; //PWM1H
			IOCON1bits.OVRDAT0=0; //PWM1L

			IOCON2bits.OVRENH=1;//Override Enable for PWM2H Pin bit 0=PWM 1= OVD
			IOCON2bits.OVRENL=1; //Override Enable for PWM2L Pin bit 0=PWM 1= OVD
			IOCON2bits.OVRDAT1=0; //PWM2H
			IOCON2bits.OVRDAT0=1; //PWM2L

			IOCON3bits.OVRENH=0;//Override Enable for PWM3H Pin bit 0=PWM 1= OVD
			IOCON3bits.OVRENL=1; //Override Enable for PWM3L Pin bit 0=PWM 1= OVD
			IOCON3bits.OVRDAT1=0; //PWM3H
			IOCON3bits.OVRDAT0=0; //PWM3L

			//A/D Trigger Select
			ADCPC0bits.TRGSRC0=0b00000;//Sample MC 
			ADCPC0bits.TRGSRC1=0b00000;//Sample MB
			ADCPC1bits.TRGSRC2=0b00000;//Sample MA

			MDC=1000;
			state=1;
		}
		else
		{
			MDC+=250;
		}
	}
	else
	{
		MDC=0;
		IOCON1bits.OVRENH=1;//Override Enable for PWM1H Pin bit 0=PWM 1= OVD
		IOCON1bits.OVRENL=1; //Override Enable for PWM1L Pin bit 0=PWM 1= OVD
		IOCON1bits.OVRDAT1=0; //PWM1H
		IOCON1bits.OVRDAT0=0; //PWM1L

		IOCON2bits.OVRENH=1;//Override Enable for PWM2H Pin bit 0=PWM 1= OVD
		IOCON2bits.OVRENL=1; //Override Enable for PWM2L Pin bit 0=PWM 1= OVD
		IOCON2bits.OVRDAT1=0; //PWM2H
		IOCON2bits.OVRDAT0=0; //PWM2L

		IOCON3bits.OVRENH=1;//Override Enable for PWM3H Pin bit 0=PWM 1= OVD
		IOCON3bits.OVRENL=1; //Override Enable for PWM3L Pin bit 0=PWM 1= OVD
		IOCON3bits.OVRDAT1=0; //PWM3H
		IOCON3bits.OVRDAT0=0; //PWM3L

		IOCON1bits.PMOD0=0;
		IOCON1bits.PMOD1=0;
		//PMOD<1:0>: PWM # I/O Pin Mode bits
		//00 = PWM I/O pin pair is in the Complementary Output mode
		//11 = PWM I/O pin pair is in the True Independent Output mode
		IOCON2bits.PMOD0=0;
		IOCON2bits.PMOD1=0;
		//PMOD<1:0>: PWM # I/O Pin Mode bits
		//00 = PWM I/O pin pair is in the Complementary Output mode
		//11 = PWM I/O pin pair is in the True Independent Output mode
		IOCON3bits.PMOD0=0;
		IOCON3bits.PMOD1=0;
		//PMOD<1:0>: PWM # I/O Pin Mode bits
		//00 = PWM I/O pin pair is in the Complementary Output mode
		//11 = PWM I/O pin pair is in the True Independent Output mode

		ADCPC0bits.TRGSRC0=0b00000; 
		ADCPC0bits.TRGSRC1=0b00000;
		ADCPC1bits.TRGSRC2=0b00000;

		//Current
		ADCPC2bits.TRGSRC4=0b00000; //Current AN9 0b10111 = PWM Generator 1 current-limit ADC trigger
		ADCPC2bits.IRQEN4=0;// Interrupt Request Enable 2 bit(2)
		//1 = Enable IRQ generation when requested conversion of channels AN9 and AN8 is completed
		//0 = IRQ is not generated
		BEMFMODE=0;
		if ( BEMFMODE==1 )
		{
			TRISAbits.TRISA1 = 0; //MAA
			TRISCbits.TRISC13=0; //MBA
			TRISCbits.TRISC0=0; //MCA
		}
		else
		{
			TRISAbits.TRISA1 = 1; //MAA
			TRISCbits.TRISC13=1; //MBA
			TRISCbits.TRISC0=1; //MCA
		}
		if ( Setpoint<MinSetpoint )
		{
			MDC=0;

			PR3=65535;
		}
		else
		{
			startcalculation=0;
			Pvalue=0;
			Ivalue=0;
			PIDset=0;

			errorcount=0;
			errorthreshold=250;

			_BEMF_BUS=200;
			MDC=0;

			CDELAY=1;
			BEMFSTARTCOUNT=0;
			PIDStartCounter=0;

			PR3=20000;

			Step=0;
			Align=1;
			//BEMF MA Rising
			IOCON1bits.OVRENH=1;//Override Enable for PWM1H Pin bit 0=PWM 1= OVD
			IOCON1bits.OVRENL=1; //Override Enable for PWM1L Pin bit 0=PWM 1= OVD
			IOCON1bits.OVRDAT1=0; //PWM1H
			IOCON1bits.OVRDAT0=0; //PWM1L

			IOCON2bits.OVRENH=1;//Override Enable for PWM2H Pin bit 0=PWM 1= OVD
			IOCON2bits.OVRENL=1; //Override Enable for PWM2L Pin bit 0=PWM 1= OVD
			IOCON2bits.OVRDAT1=0; //PWM2H
			IOCON2bits.OVRDAT0=1; //PWM2L

			IOCON3bits.OVRENH=0;//Override Enable for PWM3H Pin bit 0=PWM 1= OVD
			IOCON3bits.OVRENL=1; //Override Enable for PWM3L Pin bit 0=PWM 1= OVD
			IOCON3bits.OVRDAT1=0; //PWM3H
			IOCON3bits.OVRDAT0=0; //PWM3L

			//A/D Trigger Select
			ADCPC0bits.TRGSRC0=0b00000;//Sample MC 
			ADCPC0bits.TRGSRC1=0b00000;//Sample MB
			ADCPC1bits.TRGSRC2=0b00000;//Sample MA

			ADCPC0bits.IRQEN1=0;// Interrupt Request Enable 1 bit
			//1 = Enable IRQ generation when requested conversion of channels AN3 and AN2 is completed
			//0 = IRQ is not generated

			ADCPC0bits.IRQEN0=0;// Interrupt Request Enable 0 bit
			//1 = Enable IRQ generation when requested conversion of channels AN1 and AN0 is completed
			//0 = IRQ is not generated

			ADCPC1bits.IRQEN2=0;// Interrupt Request Enable 2 bit(2)
			//1 = Enable IRQ generation when requested conversion of channels AN5 and AN4 is completed
			//0 = IRQ is not generated
		}
	}	
}


void openloop()
{
	/* Openloop */
	Step++;
	if ( Step>6 )
	{
		Step=1;
		GLED=1;
	}
	else
	{
		GLED=0;
	}

	ADCPC0bits.TRGSRC0=0b00000;//Sample MC
	ADCPC0bits.TRGSRC1=0b00000;//Sample MB
	ADCPC1bits.TRGSRC2=0b00000;//Sample MA

	IOCON1bits.OVRENH = (Step == 2 || Step == 3) ? 0 : 1;
	IOCON1bits.OVRENL = 1;
	IOCON1bits.OVRDAT1 = 0;
	IOCON1bits.OVRDAT0 = (Step == 5 || Step == 6) ? 1 : 0;

	IOCON2bits.OVRENH = (Step == 4 || Step == 5) ? 0 : 1;
	IOCON2bits.OVRENL = 1;
	IOCON2bits.OVRDAT1 = 0;
	IOCON2bits.OVRDAT0 = (Step == 1 || Step == 2) ? 1 : 0;

	IOCON3bits.OVRENH = (Step == 6 || Step == 1) ? 0 : 1;
	IOCON3bits.OVRENL = 1;
	IOCON3bits.OVRDAT1 = 0;
	IOCON3bits.OVRDAT0 = (Step == 3 || Step == 4) ? 1 : 0;

	if (Step == 0)
	{
		MDC=0;
		state=0;
	}
	
	if ( MDC>=PWMopenloop )
	{
		if ( BEMFMODE==1 )
		{
			MDC=PWMopenloop;
			TRIG1=(PWMopenloop-200);
			TRIG2=(PWMopenloop-200);
			TRIG3=(PWMopenloop-200);
			STRIG1=PWMopenloop/2;
		}
		else
		{
			MDC=PWMopenloop;
			TRIG1=18859;
			TRIG2=18859;
			TRIG3=18859;
		}
		//state=2;
	}
	else
	{
		MDC=MDC+PWMrate;
		PR3-=PWMstartuprate;
	}
	/* EasyCODE ) */
}
